﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Forms;

using System.IO;
using AForge.Robotics.Lego;
using System.Threading;

namespace nxt
{
    public partial class ControlPanel : Form
    {
        public int[] pos = new int[4];
        public int[] motor = new int[4];
        public double height;

        public ControlPanel()
        {
            InitializeComponent();
        }

        Control robot;

        string cPTemp;
        string[] cP = new string[2];

        #region CONNECT

        private void buttonConnect_Click(object sender, EventArgs e)
        {
            StreamReader rs;

            bool nxtConnected_1 = false; //felső nxt
            bool nxtConnected_2 = false; //alsó nxt

            try
            {
                rs = new StreamReader("comPort.nxt");
                while (rs.EndOfStream == false)
                {
                    cPTemp = rs.ReadLine();
                }
                rs.Close();

                cP = cPTemp.Split(',');
            }
            catch
            {
                label8.Text = "Status: COM PORT reading error!";
            }

            robot = new Control(cP[0], cP[1]);

            label8.Text = "Status: Nxt_1 COM PORT: " + cP[0] + ", Nxt_2 COM PORT: " + cP[1];

            try
            {
                label8.Text = "Status: Connecting to Nxt_1 and Nxt_2...";
                
                robot.Connect();
                
                //felső nxt
                if (robot.nxtConnectionStatusFront() == true)
                {
                    nxtConnected_1 = true;

                    label8.Text = "Status: Nxt_1 is connected!";
                }
                else
                {
                    label8.Text = "Status: Nxt_1 is not connected!";
                }

                //alsó nxt
                if (robot.nxtConnectionStatusBack() == true)
                {
                    nxtConnected_2 = true;

                    label8.Text = "Status: Nxt_2 is connected!";
                }
                else
                {
                    label8.Text = "Status: Nxt_2 is not connected!";
                }

                //felső-alsó
                if (nxtConnected_1 == true && nxtConnected_2 == true)
                {
                    label8.Text = "Status: Connected! Ready to go!";

                    buttonConnect.Enabled = false;
                    buttonDisconnect.Enabled = true;
 
                    panel1.Enabled = true;

                    robot.greenLight();
                }
            }
            catch
            {
                label8.Text = "Status: Connection failed to Nxt!";
            }

            getBattery();

        }



        private void buttonDisconnect_Click(object sender, EventArgs e)
        {
            robot.Disconnect();

            buttonConnect.Enabled = true;
            buttonDisconnect.Enabled = false;

            panel1.Enabled = false;

            label8.Text = "Status: Disconnected!";
        }

        #endregion

        #region MOVEMENT FUNCTIONS

        private void buttonGoForward_Click(object sender, EventArgs e)
        {
            try
            {
                robot.forward(Convert.ToInt32(textBox1.Text), Convert.ToInt32(textBox2.Text), Convert.ToInt32(textBox3.Text));
            }
            catch (Exception)
            { label8.Text = "Status: Going forward failed!"; }

            getBattery();
        }

        private void buttonGoBackward_Click(object sender, EventArgs e)
        {
            try
            {
                robot.backward(Convert.ToInt32(textBox4.Text), Convert.ToInt32(textBox5.Text), Convert.ToInt32(textBox6.Text));
            }
            catch (Exception)
            { label8.Text = "Status: Going backward failed!"; }

            getBattery();
        }

        private void buttonGoRight_Click(object sender, EventArgs e)
        {
            try
            {
                robot.right(Convert.ToInt32(textBox7.Text), Convert.ToInt32(textBox8.Text), Convert.ToInt32(textBox9.Text));
            }
            catch (Exception)
            { label8.Text = "Status: Going right failed!"; }

            getBattery();
        }

        private void buttonGoLeft_Click(object sender, EventArgs e)
        {
            try
            {
                robot.left(Convert.ToInt32(textBox10.Text), Convert.ToInt32(textBox11.Text), Convert.ToInt32(textBox12.Text));
            }
            catch (Exception)
            { label8.Text = "Status: Going left failed!"; }

            getBattery();
        }

        private void buttonGoUp_Click(object sender, EventArgs e)
        {
            try
            {
                robot.up(Convert.ToInt32(textBox13.Text), Convert.ToInt32(textBox14.Text), Convert.ToInt32(textBox15.Text));
            }
            catch (Exception)
            { label8.Text = "Status: Going up failed!"; }

            getBattery();
        }

        private void buttonGoDown_Click(object sender, EventArgs e)
        {
            try
            {
                robot.down(Convert.ToInt32(textBox16.Text), Convert.ToInt32(textBox17.Text), Convert.ToInt32(textBox18.Text));
            }
            catch (Exception)
            { label8.Text = "Status: Going down failed!"; }

            getBattery();
        }

        private void buttonGoMotor_Click(object sender, EventArgs e)
        {
            switch (comboBox1.SelectedItem.ToString())
            {
                case "felső _A":
                    try
                    {
                        robot.moveFrontMotor(NXTBrick.Motor.A, Convert.ToInt32(textBox19.Text), Convert.ToInt32(textBox20.Text));
                    }
                    catch (Exception)
                    { label8.Text = "Status: Motor failed!"; }
                    break;
                case "felső _B":
                    try
                    {
                        robot.moveFrontMotor(NXTBrick.Motor.B, Convert.ToInt32(textBox19.Text), Convert.ToInt32(textBox20.Text));
                    }
                    catch (Exception)
                    { label8.Text = "Status: Motor failed!"; }
                    break;
                case "alsó _A":
                    try
                    {
                        robot.moveBackMotor(NXTBrick.Motor.A, Convert.ToInt32(textBox19.Text), Convert.ToInt32(textBox20.Text));
                    }
                    catch (Exception)
                    { label8.Text = "Status: Motor failed!"; }
                    break;
                case "alsó _B":
                    try
                    {
                        robot.moveBackMotor(NXTBrick.Motor.B, Convert.ToInt32(textBox19.Text), Convert.ToInt32(textBox20.Text));
                    }
                    catch (Exception)
                    { label8.Text = "Status: Motor failed!"; }
                    break;
            }

            getBattery();
        }

        #endregion

        #region MOVEMENT

        /// <summary>
        /// Robot mozgás vezérlése x,y,z koordinátára
        /// A mozgás 3 fázisban fut le
        /// először előre/hátra mozgást végzi el
        /// majd a jobbra-ballra
        /// végül a fel-le pozícionálást
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void NXTMovement_Click(object sender, EventArgs e)
        {
            double x = 0, y = 0, z = 0;

            try
            {
                x = Convert.ToDouble(textBox21.Text);
                y = Convert.ToDouble(textBox22.Text);
                z = Convert.ToDouble(textBox23.Text);
            }
            catch (Exception)
            { label8.Text = "Status: Reading error!"; }

            //forwardBackwordMovement(z, x);
            motor = robot.countForwardBackwordMovement(z, x, pos[0],pos[1],pos[2],pos[3]);
            
            // ha x > a akkor előre megy, ah kisebb hátra
            if (x != 0)
            {

                if (x > 0)
                {
                    robot.forward(20, 100, motor[0], motor[1], motor[2], motor[3]);
                    pos[0] = pos[0] - Convert.ToInt32((motor[0] / 5.625));
                    pos[1] = pos[1] - Convert.ToInt32((motor[1] / 5.625));
                    pos[2] = pos[2] + Convert.ToInt32((motor[2] / 5.625));
                    pos[3] = pos[3] + Convert.ToInt32((motor[3] / 5.625));
                    Thread.Sleep(robot.getSleep(Convert.ToInt32(x)));
                }
                else
                {
                    robot.backward(20, 100, motor[0], motor[1], motor[2], motor[3]);
                    pos[0] = pos[0] + Convert.ToInt32((motor[0] / 5.625));
                    pos[1] = pos[1] + Convert.ToInt32((motor[1] / 5.625));
                    pos[2] = pos[2] - Convert.ToInt32((motor[2] / 5.625));
                    pos[3] = pos[3] - Convert.ToInt32((motor[3] / 5.625));
                    Thread.Sleep(robot.getSleep(Convert.ToInt32(Math.Abs(x))));
                }
            }
            //sideMovement(z, y);
            motor = robot.countSideMovement(z, y, pos[0], pos[1], pos[2], pos[3]);
            // ha y kisebb mint 0, akkor jobbra megy, ha nagyobb balra
            if (y != 0)
            {

                if (y > 0)
                {
                    robot.right(20, 100, motor[0], motor[1], motor[2], motor[3]);
                    pos[0] = pos[0] - Convert.ToInt32((motor[0] / 5.625));
                    pos[1] = pos[1] + Convert.ToInt32((motor[1] / 5.625));
                    pos[2] = pos[2] - Convert.ToInt32((motor[2] / 5.625));
                    pos[3] = pos[3] + Convert.ToInt32((motor[3] / 5.625));
                    Thread.Sleep(robot.getSleep(Convert.ToInt32(x)));
                }
                else
                {
                    robot.left(20, 100, motor[0], motor[1], motor[2], motor[3]);
                    pos[0] = pos[0] + Convert.ToInt32((motor[0] / 5.625));
                    pos[1] = pos[1] - Convert.ToInt32((motor[1] / 5.625));
                    pos[2] = pos[2] + Convert.ToInt32((motor[2] / 5.625));
                    pos[3] = pos[3] - Convert.ToInt32((motor[3] / 5.625));
                    Thread.Sleep(robot.getSleep(Convert.ToInt32(Math.Abs(x))));
                }
            }
            //upDownMovement(z, z);
            motor = robot.countUpDownMovement(z, height, pos[0], pos[1], pos[2], pos[3]);
            // ha z kisebb mint 0 lefelé ha nagyob felfelé emgy
            if (z != 0)
            {

                if (z > 0)
                {
                    robot.up(20, 100, motor[0], motor[1], motor[2], motor[3]);
                    pos[0] = pos[0] - Convert.ToInt32((motor[0] / 5.625));
                    pos[1] = pos[1] - Convert.ToInt32((motor[1] / 5.625));
                    pos[2] = pos[2] - Convert.ToInt32((motor[2] / 5.625));
                    pos[3] = pos[3] - Convert.ToInt32((motor[3] / 5.625));
                    Thread.Sleep(robot.getSleep(Convert.ToInt32(x)));
                }
                else
                {
                    robot.down(20, 100, motor[0], motor[1], motor[2], motor[3]);
                    pos[0] = pos[0] + Convert.ToInt32((motor[0] / 5.625));
                    pos[1] = pos[1] + Convert.ToInt32((motor[1] / 5.625));
                    pos[2] = pos[2] + Convert.ToInt32((motor[2] / 5.625));
                    pos[3] = pos[3] + Convert.ToInt32((motor[3] / 5.625));
                    Thread.Sleep(robot.getSleep(Convert.ToInt32(Math.Abs(x))));
                }
            }

            getBattery();
        }
        /*
        public void forwardBackwordMovement(double height, double moveX)
        {
            //double pos[0], pos[1], pos[2], pos[3];
            pos[0] = Convert.ToInt32((Math.Abs(pos[0] - Math.Sqrt((height * height) + (pos[0] + moveX) * (pos[0] + moveX))) / 40) * 360);
            pos[1] = Convert.ToInt32((Math.Abs(pos[1] - Math.Sqrt((height * height) + (pos[1] + moveX) * (pos[1] + moveX))) / 40) * 360);
            pos[2] = Convert.ToInt32((Math.Abs(pos[2] - Math.Sqrt((height * height) + (pos[2] - moveX) * (pos[2] - moveX))) / 40) * 360);
            pos[3] = Convert.ToInt32((Math.Abs(pos[3] - Math.Sqrt((height * height) + (pos[3] - moveX) * (pos[3] - moveX))) / 40) * 360);
            //a motor értéke most már adott fordulatban
        }

        public void sideMovement(double height, double moveY)
        {
            //double pos[0], pos[1], pos[2], pos[3];
            pos[0] = Convert.ToInt32((Math.Abs(pos[0] - Math.Sqrt((height * height) + (pos[0] + moveY) * (pos[0] + moveY))) / 40) * 360);
            pos[1] = Convert.ToInt32((Math.Abs(pos[1] - Math.Sqrt((height * height) + (pos[1] - moveY) * (pos[1] - moveY))) / 40) * 360);
            pos[2] = Convert.ToInt32((Math.Abs(pos[2] - Math.Sqrt((height * height) + (pos[2] + moveY) * (pos[2] + moveY))) / 40) * 360);
            pos[3] = Convert.ToInt32((Math.Abs(pos[3] - Math.Sqrt((height * height) + (pos[3] - moveY) * (pos[3] - moveY))) / 40) * 360);
            //a motor értéke most már adott fordulatban
        }

        public void upDownMovement(double height, double moveZ)
        {
            //double pos[0], pos[1], pos[2], pos[3];
            pos[0] = Convert.ToInt32((Math.Abs(pos[0] - Math.Sqrt((height + moveZ) * (height + moveZ) + (pos[0] * pos[0]))) / 40) * 360);
            pos[1] = Convert.ToInt32((Math.Abs(pos[1] - Math.Sqrt((height + moveZ) * (height + moveZ) + (pos[1] * pos[1]))) / 40) * 360);
            pos[2] = Convert.ToInt32((Math.Abs(pos[2] - Math.Sqrt((height + moveZ) * (height + moveZ) + (pos[2] * pos[2]))) / 40) * 360);
            pos[3] = Convert.ToInt32((Math.Abs(pos[3] - Math.Sqrt((height + moveZ) * (height + moveZ) + (pos[3] * pos[3]))) / 40) * 360);
            //a motor értéke most már adott fordulatban
        }
        */
        #endregion

        /// <summary>
        /// Battery
        /// </summary>
        private void getBattery()
        {
            try
            {
                int frontmVolt;
                int backmVolt;

                robot.FrontModul.GetBatteryPower(out frontmVolt);
                robot.BackModul.GetBatteryPower(out backmVolt);
                labelBattery.Text = "Upper NXT: " + frontmVolt.ToString() + " mVolts, lower NXT: " + backmVolt.ToString()+" mVolts"; 
            }
            catch(Exception)
            {
                //
            }

            
        }

        /// <summary>
        /// Robotic arm close and open
        /// </summary>
        static bool robot_arm_close = true; 

        private void button_arm_go_Click(object sender, EventArgs e)
        {
            try
            {
                robot_arm_close = robot.robotic_arm_close_open(robot_arm_close);
                if (robot_arm_close)
                {
                    button_arm_go.Text = "Open";
                }
                else
                {
                    button_arm_go.Text = "Close";
                }
            }
            catch (Exception)
            { label8.Text = "Status: Robotic arm Motor failed!"; }
        }

        private void button17_Click(object sender, EventArgs e)
        {
            Manualy_controller MC = new Manualy_controller();
            MC.ShowDialog();
        }
        /// <summary>
        /// Damil hossz érték megadása
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void button1_Click(object sender, EventArgs e)
        {
            pos[0] = Convert.ToInt32(textBox24.Text);
            pos[1] = Convert.ToInt32(textBox25.Text);
            pos[2] = Convert.ToInt32(textBox26.Text);
            pos[3] = Convert.ToInt32(textBox27.Text);
            height = Convert.ToInt32(textBox28.Text);
        }

        private void button2_Click(object sender, EventArgs e)
        {
            try
            {
                
                
                robot.moveFrontMotor(NXTBrick.Motor.A, Convert.ToInt32(textBox29.Text), Convert.ToInt32(textBox33.Text));
                robot.moveBackMotor(NXTBrick.Motor.A, Convert.ToInt32(textBox30.Text), Convert.ToInt32(textBox34.Text));
                robot.moveFrontMotor(NXTBrick.Motor.B, Convert.ToInt32(textBox31.Text), Convert.ToInt32(textBox35.Text));
                robot.moveBackMotor(NXTBrick.Motor.B, Convert.ToInt32(textBox32.Text), Convert.ToInt32(textBox36.Text));

            }
            catch (Exception)
            {
                //
            }

        }

    
    }
}
